CON

  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000


  LEFT_HB_25 = 14
  RIGHT_HB_25 = 15
  PING_BASE_PIN = 4


obj


  ping:  "Ping"                                         'ultrasonic "ping" sensor driver library.
                                                        'Calls to ping object utilize functions in the Ping
                                                        'library.

  pst    : "Parallax Serial Terminal"                   ' Serial communication object



var

  long sensor_stack[50]
  long display_stack[50]
  

  byte pings[10]

  long  left_motor, right_motor

  byte  state, substate, oldstate, status

  'word  left_ping, right_ping, front_ping, left_45_ping, right_45_ping, bk_ping



pub go

  cognew(read_sensors_run_motors, @sensor_stack)


  repeat

    left_motor := right_motor := 1300
  

pub read_sensors

'constantly reads the pings in a round robin fasion.  Quick and dirty, this could be
'cleaned up.  For now though, we have PLENTY of available space so we can be loose with
'variables. 
  
{
  repeat
    front_ping := eyes[2] := ping.inches(F_PING_PIN)
    pause(1_000)
    left_ping := eyes[0] := ping.inches(L_PING_PIN)
    pause(1_000)
    right_ping := eyes[4] := ping.inches(R_PING_PIN)
    pause(1_000)
    left_45_ping := eyes[1] := ping.inches(L45_PING_PIN)
    pause(1_000)
    right_45_ping := eyes[3] := ping.inches(R45_PING_PIN)
    pause(1_000)
    bk_ping := eyes[5] := ping.inches(BK_PING_PIN)
    pause(1_000)    

}

pub read_sensors_run_motors | i, base_pin

'constantly reads the pings in a round robin fasion.  Quick and dirty, this could be
'cleaned up.  For now though, we have PLENTY of available space so we can be loose with
'variables. 




  repeat
    'repeat i from 0 to 9
      i++
      
      pings[0] := ping.inches(ping_base_pin + 0)
      pause(1_000)
      pings[1] := ping.inches(ping_base_pin + 2)
      pause(1_000)
      pings[2] := ping.inches(ping_base_pin + 4)
      pause(1_000)
      'pings[3] := ping.inches(ping_base_pin + 5)
      pause(1_000)
      'pings[4] := ping.inches(ping_base_pin + 7)
      pause(1_000)
      'pings[5] := ping.inches(ping_base_pin + 9)
      
      pause(1_000)

      pulsout(LEFT_HB_25, left_motor)
      pulsout(RIGHT_HB_25, right_motor)
    


pri Pause(duration)

  'This function pauses program execution for approximately [duration] micro seconds
  'so 1_000_000 would be approximately one second.  Doesnt account for instruction
  'execution time overhead.

  if duration < 381
    duration := 381             'lower bound limit. anything lower than this, we
                                'have to wait until the counter comes back around,
                                'which is MUCH longer than [duration].
  waitcnt(((clkfreq / 1_000_000) * duration) + cnt)


pri Future_cnt(usec)            'simple method to return the count for some usec in the future
  return(((clkfreq / 1_000_000) * usec) + cnt)

  
pri PulsOut(pin, duration)

  'similar to the BS2 command.  This function pulses the [pin] for [duration].
  'This is extremely useful for controlling servos and speed controllers.
  'To constantly make a servo move, you have to constantly be calling this
  'function (I.E. you cant set a value and have the servo stay at that value)

  dira[pin]~~      'make pin an output
  outa[pin]~~      'make pin go high            
  waitcnt(((clkfreq / 1_000_000) * duration) + cnt)  'pause execution
  outa[pin]~       'make pin go low    